#include <iostream>
#include "geo_api.h"

Cgeo_API geo;

int execute_geo_command(unsigned char command, void *buf, int size);
unsigned int peek(unsigned int addr);
unsigned int poke(unsigned int addr, unsigned int data);

unsigned int peek(unsigned int addr)
{
unsigned int data;

    memcpy(&geo.geo_buff[1], &addr, sizeof(unsigned int));
    execute_geo_command(GEO_PEEK, geo.geo_buff, 64);
    memcpy(&data, geo.geo_buff, sizeof(unsigned int));

    return data;
}

unsigned int poke(unsigned int addr, unsigned int data)
{
    memcpy(&geo.geo_buff[1], &addr, sizeof(unsigned int));
    memcpy(&geo.geo_buff[sizeof(unsigned int)+1], &data, sizeof(unsigned int));

    execute_geo_command(GEO_POKE, geo.geo_buff, 64);

    return 0;
}

int execute_geo_command(unsigned char command, void *buf, int size)
{
    //NOTE: Every GEO command sent to the target device will return a status packet immediately after it has completed execution. The 'size' parameter defines the number of bytes expected from the GEO target device after reception of this status packet.
unsigned int fw_status = 0x00000000; //FW failure if this is 0xFFFFFFFF
int num_bytes=0, retval;

	//memset(geo_buff, 0x00, 64);
	geo.geo_buff[0] = command;

	retval = geo.Process_Module_API_Request(GEO_API_WRITE, 0x00, geo.geo_buff, 64);

	if(retval){
		printf("%s:GEO_API_WRITE FAILED with error code %d\n", __FUNCTION__, retval);
	}else{
        if(size == 0){
            retval = geo.Process_Module_API_Request(GEO_API_READ, 0x00, geo.geo_buff, 64);

            if(retval){
                printf("%s:GEO_API_READ FAILED with error code %d\n", __FUNCTION__, retval);
            }else{
			    //printf("%s:GEO_API_READ SUCCESS %d\n", __FUNCTION__, retval);
            }
        }else{
            do{
                if(size < 64)
                    num_bytes = size;
                else
                    num_bytes = 64;

                retval = geo.Process_Module_API_Request(GEO_API_READ, 0x00, geo.geo_buff, num_bytes);

                if(retval){
                    printf("%s:GEO_API_READ FAILED with error code %d\n", __FUNCTION__, retval);
                    exit(-1);
                }else{
			        printf("%s:GEO_API_READ SUCCESS %d\n", __FUNCTION__, retval);
                }

            }while(size != 0);

        }
	}

    if(fw_status == 0xFFFFFFFF){
		cout << "ERROR: FW returned 0xffffffff error code" << endl;
		exit(-5);
	}

    return retval;
}

using namespace std;
unsigned int GPIO[5][9]; //9 registers per gpio port

int main()
{
    cout << "STM32 Example" << endl;

    for(unsigned int i=0; i<9; i++){
        GPIO[0][i] = peek(0x40020000 + i); //GPIOA_BASE
        GPIO[1][i] = peek(0x40020400 + i); //GPIOB_BASE
        GPIO[2][i] = peek(0x40020800 + i); //GPIOC_BASE
        GPIO[3][i] = peek(0x40020C00 + i); //GPIOD_BASE
        GPIO[4][i] = peek(0x40021000 + i); //GPIOE_BASE
    }

    return 0;
}
